﻿
// 2020/1/16: 首个记录，基于V3 2019/10/11
// 2020/5/15: 移除编译warning
// 2020/6/17: public ID改为raw ID
// 2020/7/20: 更新样本标题
// 2020/9/29: 修正TTLC计算
// 2020/11/12: 支持Spadas7
// 2022/11/2: 支持Spadas8.4
// 2023/10/11: 补充部分英文注释
// 2024/7/3: 基于新样本框架。枚举改为使用enum class
// 2025/3/31: 实现插值
// 2025/8/4: 不使用cat宏
// 2025/8/4: 改为继承SessionSample。改回单头文件

#ifndef LANE_SENSOR_SAMPLE_V4_H
#define LANE_SENSOR_SAMPLE_V4_H

#include "spadas.h"

namespace lane_sensor_sample_v4
{
	using namespace spadas;

	enum class LineDescription
	{
		Model = 1, // Describe with model coefficients and consistent properties / 通过模型参数和一致的属性进行描述
		Segments = 2, // Describe with many segments and properties of each segment / 通过多段线段和各自的属性进行描述
		Both = 3, // Both model and segments are supported / Model和Segments双模式描述
	};

	enum class LineClass
	{
		General = 1, // 未指定
		Dashed = 2, // 虚线
		Solid = 3, // 实线
		DoubleDashed = 4, //双虚线
		DoubleSolid = 5, //双实线
		SolidDashed = 6, // 左实右虚
		DashedSolid = 7, // 左虚右实
		Curb = 8, // 路沿
		Guardrail = 9, // 护栏
		Barrier = 10, //障碍物边界
	};

	enum class LineColor
	{
		Unknown = 1, // 未知
		White = 2, // 白色
		Yellow = 3, // 黄色
		Red = 4, // 红色
		Blue = 5, // 蓝色
		Orange = 6, // 橙色
		Green = 7, // 绿色
	};

	struct LineSegmentSolidProperty
	{
		Bool valid; // Whether it's valid, FALSE indicates virtual part / 是否有效，FALSE表示为虚线段
		LineColor color; // Color of the line segment / 线段颜色
		OptionalDouble width; // [m] Width of the line segment / 线段宽度

		LineSegmentSolidProperty()
		{
			valid = FALSE;
			color = LineColor::Unknown;
		}
	};

	struct LineSegment
	{
		Int id; // ID of line segment / 线段ID
		Point2D p1; // [m] x-y coordination of the start point / 线段首端点的x-y坐标
		OptionalDouble z1; // [m] z coordination of the start point / 线段首端点的z坐标
		Point2D p2; // [m] x-y coordination of the end point / 线段末端点的x-y坐标
		OptionalDouble z2; // [m] z coordination of the end point / 线段末端点的z坐标
		LineSegmentSolidProperty solidProperty; // Properties of solid line segment (Not for virtual part) / 实线段的属性（虚线段则为空）
	};

	struct LaneLine
	{
		// Basic info / 基本信息
		Int id; // ID of the lane line, zero as invalid / 车道线ID，0表示无效
		OptionalInt rawID; // Raw ID of the lane line / 车道线原始ID
		OptionalDouble confidence; // [%] Confidence of the lane line / 车道线置信度
		LineDescription description; // How to describe the lane line's curve / 车道线的描述方式

		// Model description / 模型描述
		LineClass clas; // Model description: Classification of lane line / Model模式：一致性车道线类型
		LineColor color; // Model description: Color of lane line / Model模式：一致性车道线颜色
		OptionalDouble width; // [m] Model description: Line width of lane line / Model模式：一致性车道线宽度
		Double param[4]; // Model description / Model模式：y = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
		Bool paramZValid; // Model description: Whether z-function is valid / Model模式：z方程是否有效
		Double paramZ[4]; // Model description / Model模式：z = coef[0] + coef[1] * x + coef[2] * x^2 + coef[3] * x^3
		OptionalDouble rearEnd; // [m] Model description: X-axis coordination of read end of the lane line / Model模式：曲线后端点位置(x轴)
		OptionalDouble frontEnd; // [m] Model description: X-axis coordination of front end of the lane line / Model模式：曲线前端点位置(x轴)

		// Segment description / 线段描述
		Array<LineSegment> segments; // Segment description: List of line segments / Segments模式：线段列表

		LaneLine()
		{
			id = -1;
			description = LineDescription::Model;
			clas = LineClass::General;
			color = LineColor::Unknown;
			param[0] = param[1] = param[2] = param[3] = 0;
			paramZValid = FALSE;
			paramZ[0] = paramZ[1] = paramZ[2] = paramZ[3] = 0;
		}

		Bool modelValid()
		{
			return description == LineDescription::Model || description == LineDescription::Both;
		}

		Bool segmentsValid()
		{
			return description == LineDescription::Segments || description == LineDescription::Both;
		}
	};

	struct LaneSensorSampleData
	{
		// Lane lines / 车道线
		Array<LaneLine> lines; // List of lane lines / 车道线列表
		OptionalInt firstLeftLine; // Index of the first lane line to the left side / 左侧第一条车道线序号
		OptionalInt firstRightLine; // Index of the first lane line to the right side / 右侧第一条车道线序号
		OptionalInt secondLeftLine; // Index of the second lane line to the left side / 左侧第二条车道线序号
		OptionalInt secondRightLine; // Index of the second lane line to the right side / 右侧第二条车道线序号

		// Lane properties / 道路属性部分
		OptionalDouble laneWidth; // [m] Current lane's width / 当前车道宽度
		OptionalDouble laneHeading; // [deg] Current lane's orientation / 当前车道朝向
		OptionalDouble laneCurvature; // [1/m] Current lane's curvature / 当前车道曲率

		// Driving status along current lane / 本车道行驶状态
		OptionalDouble lateralVelocity; // [m/s] Lateral speed to current lane / 当前车道横向速度
		OptionalDouble centerDeparture; // [m] Central departure to current lane / 当前车道中央偏离距离

		// Subject vehicle info / 车辆参数部分
		OptionalDouble vehiVX; // [kph] Subject vehicle's speed / 本车车速
		OptionalDouble vehiCurv; // [1/m] Subject vehicle's turning curvature / 本车行驶曲率
		OptionalDouble vehiAX; // [m/s²] Subject vehicle's longitudinal acceleration / 本车纵向加速度
		OptionalDouble vehiWidth; // [m] Subject vehicle's width / 本车宽度
		OptionalDouble vehiOverhang; // [m] Subject vehicle's front overhang / 本车前悬
		OptionalDouble vehiWheelBase; // [m] Subject vehicle's wheel base / 本车轴距

		// Sensor detect range / 车道线识别边界
		Double rearBound; // [m] Rear bound of detect range / 传感器检测范围后边界
		Double frontBound; // [m] Front bound of detect range / 传感器检测范围前边界

		LaneSensorSampleData()
		{
			rearBound = 0;
			frontBound = 30;
		}

		OptionalDouble getDTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiWidth.valid && vehiOverhang.valid)
			{
				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double x = -1 * vehiOverhang.value;
				Double y = (values[3] * x * x * x + values[2] * x * x
					+ values[1] * x + values[0]) * (isLeft ? 1 : -1);
				Double cosTheta = math::abs(math::cos(math::atan(values[1])));
				return cosTheta * (y - vehiWidth.value / 2.0);
			}
			else return OptionalDouble();
		}

		OptionalDouble getSTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiVX.valid)
			{
				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double curv = vehiCurv.valid ? vehiCurv.value : 0;
				Double fo = vehiOverhang.valid ? vehiOverhang.value : 0.9;
				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;

				Double sinTheta = math::sin(math::atan(values[1] - curv * (fo + wb)));
				return (isLeft ? -1.0 : 1.0) * sinTheta * vehiVX.value / 3.6;
			}
			else return OptionalDouble();
		}

		OptionalDouble getTTLC(LaneLine& line)
		{
			if (line.modelValid() && vehiVX.valid && vehiWidth.valid && vehiOverhang.valid)
			{
				if (vehiVX.value <= 0) return OptionalDouble();

				Double *values = line.param;
				Bool isLeft = values[0] > 0;

				Double wb = vehiWheelBase.valid ? vehiWheelBase.value : 2.8;
				Double x0 = -vehiOverhang.value - wb;
				Double x = x0;
				Double y = 0;
				Double vx = vehiVX.value / 3.6;

				UInt loopTimes = vx < 5 ? 1000 : (UInt)(50.0 / vx * 100); // No output if crossing beyond 50m (Can't guarantee precision) / 超过50米越线则不输出（精度无法保证）

				Double rx = wb;
				Double ry = (isLeft ? 0.5 : -0.5) * vehiWidth.value;

				for (UInt i = 0; i <= loopTimes; i++)
				{
					Double k = 0;
					if (vehiCurv.valid) k = vehiCurv.value * (x - x0);

					Double cosa = 1.0 / math::abs(1 + k);
					Double sina = cosa * k;

					Double tx = x + cosa * rx - sina * ry;
					Double ty = y + sina * rx + cosa * ry;
					Double ly = values[3] * tx * tx * tx + values[2] * tx * tx + values[1] * tx + values[0];

					Bool passed = FALSE;
					if (isLeft)
					{
						if (ly <= ty) passed = TRUE;
					}
					else
					{
						if (ly >= ty) passed = TRUE;
					}
					if (passed)
					{
						if (i == 0) return OptionalDouble();
						else return (Double)i / 100;
					}

					if (vehiAX.valid) vx += vehiAX.value * 0.01;
					if (vx <= 0) break;

					x += vx * 0.01 * cosa;
					y += vx * 0.01 * sina;
				}

				return OptionalDouble();
			}
			else return OptionalDouble();
		}
	};

	class LaneSensorSample : public SessionSample
	{
	private:
		class LaneSensorSampleVars : public SessionSampleVars
		{
		public:
			SPADAS_VARS(LaneSensorSample, SessionSampleVars)
			LaneSensorSampleData data;

			SessionGeneralSample toGeneralSample()
			{
				SessionGeneralSample sample;
				sample.timestamp() = timestamp;

				UInt segmentCount = 0;
				Array<UInt> segmentOffset(data.lines.size());
				Array<UInt> segmentSize(data.lines.size());
				for (UInt i = 0; i < data.lines.size(); i++)
				{
					segmentOffset[i] = segmentCount;
					segmentSize[i] = data.lines[i].segments.size();
					segmentCount += segmentSize[i];
				}

				Array<GeneralElement> values(23 + data.lines.size() * 27 + segmentCount * 12);
				sample.values() = values;

				values[0] = data.lines.size();
				values[1] = segmentCount;

				values[2] = data.lateralVelocity;
				values[3] = data.centerDeparture;
				values[4] = data.laneWidth;
				values[5] = data.laneHeading;
				values[6] = data.laneCurvature;
				values[7] = data.rearBound;
				values[8] = data.frontBound;
				values[9] = data.firstLeftLine.valid ? data.firstLeftLine.value : OptionalDouble();
				values[10] = data.firstRightLine.valid ? data.firstRightLine.value : OptionalDouble();
				values[11] = data.secondLeftLine.valid ? data.secondLeftLine.value : OptionalDouble();
				values[12] = data.secondRightLine.valid ? data.secondRightLine.value : OptionalDouble();
				values[13] = data.vehiVX;
				values[14] = data.vehiCurv;
				values[15] = data.vehiAX;
				values[16] = data.vehiWidth;
				values[17] = data.vehiOverhang;
				values[18] = data.vehiWheelBase;

				UInt segBase = 23 + data.lines.size() * 27;

				for (UInt i = 0; i < data.lines.size(); i++)
				{
					LaneLine& line = data.lines[i];
					UInt baseIndex = 23 + i * 27;

					values[baseIndex + 0] = (Double)line.id;
					values[baseIndex + 1] = line.rawID.valid ? (Double)line.rawID.value : OptionalDouble();
					values[baseIndex + 2] = line.confidence;
					values[baseIndex + 3] = (Double)(Int)line.description;

					if (line.modelValid())
					{
						values[baseIndex + 4] = (Double)(Int)line.clas;
						values[baseIndex + 5] = (Double)(Int)line.color;
						values[baseIndex + 6] = line.width;
						values[baseIndex + 7] = line.param[0];
						values[baseIndex + 8] = line.param[1];
						values[baseIndex + 9] = line.param[2];
						values[baseIndex + 10] = line.param[3];
						values[baseIndex + 11] = line.paramZValid ? 1.0 : 0.0;
						values[baseIndex + 12] = line.paramZ[0];
						values[baseIndex + 13] = line.paramZ[1];
						values[baseIndex + 14] = line.paramZ[2];
						values[baseIndex + 15] = line.paramZ[3];
						values[baseIndex + 16] = line.rearEnd;
						values[baseIndex + 17] = line.frontEnd;
					}

					if (line.segmentsValid())
					{
						UInt segOffset = segmentOffset[i];
						UInt segSize = segmentSize[i];
						values[baseIndex + 25] = (Double)segOffset;
						values[baseIndex + 26] = (Double)segSize;
						
						if (segSize > 0)
						{
							for (UInt n = 0; n < segSize; n++)
							{
								LineSegment& seg = line.segments[n];
								UInt segBaseIndex = segBase + 12 * (segOffset + n);

								values[segBaseIndex + 0] = (Double)seg.id;
								values[segBaseIndex + 1] = seg.p1.x;
								values[segBaseIndex + 2] = seg.p1.y;
								values[segBaseIndex + 3] = seg.z1;
								values[segBaseIndex + 4] = seg.p2.x;
								values[segBaseIndex + 5] = seg.p2.y;
								values[segBaseIndex + 6] = seg.z2;
								values[segBaseIndex + 7] = seg.solidProperty.valid ? 1.0 : 0.0;
								values[segBaseIndex + 8] = (Double)(Int)seg.solidProperty.color;
								values[segBaseIndex + 9] = seg.solidProperty.width;
							}
						}
					}

					values[baseIndex + 21] = data.getDTLC(line);
					values[baseIndex + 22] = data.getSTLC(line);
					values[baseIndex + 23] = data.getTTLC(line);
				}

				sample.significantCount() = values.size();
				return sample;
			}

			Bool fromSample(String protocol, SessionSample sample) override
			{
				SessionGeneralSample generalSample;
				if (!sample.is<SessionGeneralSample>(generalSample)) return FALSE;

				timestamp = generalSample.timestamp();
				auto values = generalSample.values();

				if (protocol == "lane-sensor-sample-v4")
				{
					if (values.size() < 23) return FALSE;
					if (!values[0].valid || !values[1].valid) return FALSE;

					Int lineCount = (Int)values[0].value;
					Int segmentCount = (Int)values[1].value;
					if ((Int)values.size() != 23 + lineCount * 27 + segmentCount * 12) return FALSE;

					data.lateralVelocity = values[2];
					data.centerDeparture = values[3];
					data.laneWidth = values[4];
					data.laneHeading = values[5];
					data.laneCurvature = values[6];
					data.rearBound = values[7].valid ? values[7].value : 0;
					data.frontBound = values[8].valid ? values[8].value : 30;
					data.firstLeftLine = values[9].valid ? (Int)values[9].value : OptionalInt();
					data.firstRightLine = values[10].valid ? (Int)values[10].value : OptionalInt();
					data.secondLeftLine = values[11].valid ? (Int)values[11].value : OptionalInt();
					data.secondRightLine = values[12].valid ? (Int)values[12].value : OptionalInt();
					data.vehiVX = values[13];
					data.vehiCurv = values[14];
					data.vehiAX = values[15];
					data.vehiWidth = values[16];
					data.vehiOverhang = values[17];
					data.vehiWheelBase = values[18];

					Int segBase = 23 + lineCount * 27;

					data.lines = Array<LaneLine>(lineCount);
					for (int i = 0; i < lineCount; i++)
					{
						LaneLine& line = data.lines[i];
						UInt baseIndex = 23 + 27 * i;

						line.id = (Int)values[baseIndex + 0].value;
						line.rawID = values[baseIndex + 1].valid ? (Int)values[baseIndex + 1].value : OptionalInt();
						line.confidence = values[baseIndex + 2];
						line.description = (LineDescription)(Int)values[baseIndex + 3].value;

						if (line.modelValid())
						{
							line.clas = (LineClass)(Int)values[baseIndex + 4].value;
							line.color = (LineColor)(Int)values[baseIndex + 5].value;
							line.width = values[baseIndex + 6];
							line.param[0] = values[baseIndex + 7].value;
							line.param[1] = values[baseIndex + 8].value;
							line.param[2] = values[baseIndex + 9].value;
							line.param[3] = values[baseIndex + 10].value;
							line.paramZValid = values[baseIndex + 11].value == 1;
							if (line.paramZValid)
							{
								line.paramZ[0] = values[baseIndex + 12].value;
								line.paramZ[1] = values[baseIndex + 13].value;
								line.paramZ[2] = values[baseIndex + 14].value;
								line.paramZ[3] = values[baseIndex + 15].value;
							}
							line.rearEnd = values[baseIndex + 16];
							line.frontEnd = values[baseIndex + 17];
						}

						if (line.segmentsValid())
						{
							Int segOffset = (Int)values[baseIndex + 25].value;
							Int segSize = (Int)values[baseIndex + 26].value;
							if (segSize > 0)
							{
								line.segments = Array<LineSegment>(segSize);
								for (Int n = 0; n < segSize; n++)
								{
									LineSegment& seg = line.segments[n];
									Int segBaseIndex = segBase + 12 * (segOffset + n);

									seg.id = (Int)values[segBaseIndex + 0].value;
									seg.p1.x = (Float)values[segBaseIndex + 1].value;
									seg.p1.y = (Float)values[segBaseIndex + 2].value;
									seg.z1 = values[segBaseIndex + 3];
									seg.p2.x = (Float)values[segBaseIndex + 4].value;
									seg.p2.y = (Float)values[segBaseIndex + 5].value;
									seg.z2 = values[segBaseIndex + 6];
									seg.solidProperty.valid = values[segBaseIndex + 7].value == 1;
									if (seg.solidProperty.valid)
									{
										seg.solidProperty.color = (LineColor)(Int)values[segBaseIndex + 8].value;
										seg.solidProperty.width = values[segBaseIndex + 9];
									}
								}
							}
						}
					}

					return TRUE;
				}
				else if (protocol == "lane-sensor-sample-v3")
				{
					if (values.size() < 17) return FALSE;
					if (!values[0].valid) return FALSE;

					int lineCount = (int)values[0].value;
					if ((Int)values.size() != 17 + lineCount * 15) return FALSE;

					data.lateralVelocity = values[1];
					data.centerDeparture = values[2];
					data.laneWidth = values[3];
					data.laneHeading = values[4];
					data.laneCurvature = values[5];
					data.vehiVX = values[6];
					data.vehiCurv = values[7];
					data.vehiAX = values[8];
					data.vehiWidth = values[9];
					data.vehiOverhang = values[10];
					data.rearBound = values[11].valid ? values[11].value : 0;
					data.frontBound = values[12].valid ? values[12].value : 30;
					data.firstLeftLine = values[13].valid ? (Int)values[13].value : OptionalInt();
					data.firstRightLine = values[14].valid ? (Int)values[14].value : OptionalInt();
					data.secondLeftLine = values[15].valid ? (Int)values[15].value : OptionalInt();
					data.secondRightLine = values[16].valid ? (Int)values[16].value : OptionalInt();

					data.lines = Array<LaneLine>(lineCount);
					for (int i = 0; i < lineCount; i++)
					{
						LaneLine& line = data.lines[i];
						UInt baseIndex = 17 + 15 * i;

						line.id = (Int)values[baseIndex + 0].value;
						line.rawID = values[baseIndex + 1].valid ? (Int)values[baseIndex + 1].value : OptionalInt();
						line.clas = (LineClass)(Int)values[baseIndex + 2].value;
						line.color = (LineColor)(Int)values[baseIndex + 3].value;
						line.confidence = values[baseIndex + 4];
						line.width = values[baseIndex + 5];
						line.param[0] = values[baseIndex + 6].value;
						line.param[1] = values[baseIndex + 7].value;
						line.param[2] = values[baseIndex + 8].value;
						line.param[3] = values[baseIndex + 9].value;
						line.rearEnd = values[baseIndex + 10];
						line.frontEnd = values[baseIndex + 11];
					}

					return TRUE;
				}
				else return FALSE;
			}

			Bool supportInterpolation() override
			{
				return TRUE;
			}

			SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
			{
				LaneSensorSample ls1, ls2;
				if (!ss1.is<LaneSensorSample>(ls1) || !ss2.is<LaneSensorSample>(ls2)) return SessionSample();

				LaneSensorSampleData& s1 = ls1.var()->data;
				LaneSensorSampleData& s2 = ls2.var()->data;
				
				// 检查线条是否有有效ID
				Bool noID = FALSE;
				for (auto line = s1.lines.firstElem(); line.valid(); ++line)
				{
					if (line->id <= 0)
					{
						noID = TRUE;
						break;
					}
				}
				
				for (auto line = s2.lines.firstElem(); line.valid(); ++line)
				{
					if (line->id <= 0)
					{
						noID = TRUE;
						break;
					}
				}
				
				// 如果没有有效ID，选择权重更大的那个
				if (noID)
				{
					return w1 > w2 ? ss1 : ss2;
				}
				
				// 检查左右线是否匹配
				OptionalInt firstLeftID, firstRightID;
				if (s1.firstLeftLine.valid && s2.firstLeftLine.valid && 
					s1.lines[s1.firstLeftLine.value].id == s2.lines[s2.firstLeftLine.value].id)
				{
					firstLeftID = s1.lines[s1.firstLeftLine.value].id;
				}
				
				if (s1.firstRightLine.valid && s2.firstRightLine.valid && 
					s1.lines[s1.firstRightLine.value].id == s2.lines[s2.firstRightLine.value].id)
				{
					firstRightID = s1.lines[s1.firstRightLine.value].id;
				}
				
				OptionalInt secondLeftID, secondRightID;
				if (s1.secondLeftLine.valid && s2.secondLeftLine.valid && 
					s1.lines[s1.secondLeftLine.value].id == s2.lines[s2.secondLeftLine.value].id)
				{
					secondLeftID = s1.lines[s1.secondLeftLine.value].id;
				}
				
				if (s1.secondRightLine.valid && s2.secondRightLine.valid && 
					s1.lines[s1.secondRightLine.value].id == s2.lines[s2.secondRightLine.value].id)
				{
					secondRightID = s1.lines[s1.secondRightLine.value].id;
				}
				
				// 创建输出对象
				LaneSensorSample output;
				output.timestamp() = timestamp;
				LaneSensorSampleData& so = output.var()->data;
				
				// 插值基本数据
				so.lateralVelocity = s1.lateralVelocity * w1 + s2.lateralVelocity * w2;
				so.centerDeparture = s1.centerDeparture * w1 + s2.centerDeparture * w2;
				
				so.laneWidth = s1.laneWidth * w1 + s2.laneWidth * w2;
				so.laneCurvature = s1.laneCurvature * w1 + s2.laneCurvature * w2;
				so.laneHeading = s1.laneHeading * w1 + s2.laneHeading * w2;
				
				so.vehiVX = s1.vehiVX * w1 + s2.vehiVX * w2;
				so.vehiCurv = s1.vehiCurv * w1 + s2.vehiCurv * w2;
				so.vehiAX = s1.vehiAX * w1 + s2.vehiAX * w2;
				so.vehiWidth = s1.vehiWidth;
				so.vehiOverhang = s1.vehiOverhang;
				so.vehiWheelBase = s1.vehiWheelBase;
				
				so.rearBound = w1 > w2 ? s1.rearBound : s2.rearBound;
				so.frontBound = w1 > w2 ? s1.frontBound : s2.frontBound;
				
				// 合并车道线 - 使用ArrayX动态添加
				ArrayX<LaneLine> linesX;
				
				for (auto s1Line = s1.lines.firstElem(); s1Line.valid(); ++s1Line)
				{
					for (auto s2Line = s2.lines.firstElem(); s2Line.valid(); ++s2Line)
					{
						if (s1Line->id == s2Line->id && s1Line->description == s2Line->description)
						{
							// 创建插值线
							LaneLine line;
							line.id = s1Line->id;
							line.rawID = s1Line->rawID;
							line.confidence = s1Line->confidence * w1 + s2Line->confidence * w2;
							line.description = s1Line->description;
							
							if (line.modelValid())
							{
								line.clas = w1 > w2 ? s1Line->clas : s2Line->clas;
								line.color = w1 > w2 ? s1Line->color : s2Line->color;
								line.width = s1Line->width * w1 + s2Line->width * w2;
								line.param[0] = s1Line->param[0] * w1 + s2Line->param[0] * w2;
								line.param[1] = s1Line->param[1] * w1 + s2Line->param[1] * w2;
								line.param[2] = s1Line->param[2] * w1 + s2Line->param[2] * w2;
								line.param[3] = s1Line->param[3] * w1 + s2Line->param[3] * w2;
								
								if (s1Line->paramZValid && s2Line->paramZValid)
								{
									line.paramZValid = TRUE;
									line.paramZ[0] = s1Line->paramZ[0] * w1 + s2Line->paramZ[0] * w2;
									line.paramZ[1] = s1Line->paramZ[1] * w1 + s2Line->paramZ[1] * w2;
									line.paramZ[2] = s1Line->paramZ[2] * w1 + s2Line->paramZ[2] * w2;
									line.paramZ[3] = s1Line->paramZ[3] * w1 + s2Line->paramZ[3] * w2;
								}
								
								line.rearEnd = s1Line->rearEnd * w1 + s2Line->rearEnd * w2;
								line.frontEnd = s1Line->frontEnd * w1 + s2Line->frontEnd * w2;
							}
							
							if (line.segmentsValid())
							{
								// 使用ArrayX处理线段
								ArrayX<LineSegment> segmentsX;
								
								for (auto s1Seg = s1Line->segments.firstElem(); s1Seg.valid(); ++s1Seg)
								{
									for (auto s2Seg = s2Line->segments.firstElem(); s2Seg.valid(); ++s2Seg)
									{
										if (s1Seg->id == s2Seg->id)
										{
											LineSegment seg;
											seg.id = s1Seg->id;
											seg.p1.x = (Float)(s1Seg->p1.x * w1 + s2Seg->p1.x * w2);
											seg.p1.y = (Float)(s1Seg->p1.y * w1 + s2Seg->p1.y * w2);
											seg.z1 = s1Seg->z1 * (Float)w1 + s2Seg->z1 * (Float)w2;
											seg.p2.x = (Float)(s1Seg->p2.x * w1 + s2Seg->p2.x * w2);
											seg.p2.y = (Float)(s1Seg->p2.y * w1 + s2Seg->p2.y * w2);
											seg.z2 = s1Seg->z2 * (Float)w1 + s2Seg->z2 * (Float)w2;
											
											// 对于SolidProperty，取权重大的那个
											seg.solidProperty = w1 > w2 ? s1Seg->solidProperty : s2Seg->solidProperty;
											
											segmentsX.append(seg);
											break;
										}
									}
								}
								
								// 转换为Array
								line.segments = segmentsX.toArray();
							}
							
							// 在添加前记录索引
							Int index = linesX.size();
							if (firstLeftID.valid && firstLeftID.value == s1Line->id) so.firstLeftLine = index;
							if (firstRightID.valid && firstRightID.value == s1Line->id) so.firstRightLine = index;
							if (secondLeftID.valid && secondLeftID.value == s1Line->id) so.secondLeftLine = index;
							if (secondRightID.valid && secondRightID.value == s1Line->id) so.secondRightLine = index;
							
							linesX.append(line);
							break;
						}
					}
				}
				
				// 转换为Array
				so.lines = linesX.toArray();
				return output.as<SessionSample>();
			}
		};

	public:
		SPADAS_CLASS("lane_sensor_sample_v4.LaneSensorSample", LaneSensorSampleVars)

		LaneSensorSample() : SessionSample(new LaneSensorSampleVars)
		{}
		LaneSensorSampleData *operator ->()
		{
			return &var()->data;
		}

		static String getProtocolName(UInt channel)
		{
			return (String)"lane-sensor-sample-v4@" + channel;
		}
		SessionGeneralSample toGeneralSample()
		{
			return var()->toGeneralSample();
		}

		static Array<String> getProtocolNames(UInt channel)
		{
			return Array<String>::create(2, "lane-sensor-sample-v3", "lane-sensor-sample-v4").convert<String>([channel](auto& s){ return s + "@" + channel; });
		}
		Bool fromGeneralSample(String protocol, SessionGeneralSample generalSample)
		{
			return fromSample(protocol, generalSample);
		}
	};
}

#endif
